
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       takeoff.c
  * @author     baiyang
  * @date       2021-9-3
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// start takeoff to specified altitude above home in centimeters
void takeoff_start(UserTakeOff* takeoff, float alt_cm)
{
    // indicate we are taking off
    fms_set_land_complete(false);
    // tell position controller to reset alt target and reset I terms
    mode_set_throttle_takeoff();

    // initialise takeoff state
    takeoff->_running = true;
    takeoff->take_off_start_alt = posctrl_get_pos_target_z_cm(fms.pos_control);
    takeoff->take_off_complete_alt  = takeoff->take_off_start_alt + alt_cm;
}

// do_pilot_takeoff - controls the vertical position controller during the process of taking off
//  take off is complete when the vertical target reaches the take off altitude.
//  climb is cancelled if pilot_climb_rate_cm becomes negative
//  sets take off to complete when target altitude is within 1% of the take off altitude
void takeoff_do_pilot_takeoff(UserTakeOff* takeoff, float pilot_climb_rate_cm)
{
    // return pilot_climb_rate if take-off inactive
    if (!takeoff->_running) {
        return;
    }

    float pos_z = takeoff->take_off_complete_alt;
    float vel_z = pilot_climb_rate_cm;

    // command the aircraft to the take off altitude and current pilot climb rate
    posctrl_input_pos_vel_accel_z(fms.pos_control, &pos_z, &vel_z, 0, true);

    // stop take off early and return if negative climb rate is commanded or we are within 0.1% of our take off altitude
    if (math_flt_negative(pilot_climb_rate_cm) ||
        (takeoff->take_off_complete_alt  - takeoff->take_off_start_alt) * 0.999f < posctrl_get_pos_target_z_cm(fms.pos_control) - takeoff->take_off_start_alt) {
        takeoff_stop(takeoff);
    }
}

bool takeoff_triggered(const float target_climb_rate)
{
    if (!fms.ap.land_complete) {
        // can't take off if we're already flying
        return false;
    }
    if (target_climb_rate <= 0.0f) {
        // can't takeoff unless we want to go up...
        return false;
    }

    if (fms.motors->_spool_state != MOTOR_THROTTLE_UNLIMITED) {
        // hold aircraft on the ground until rotor speed runup has finished
        return false;
    }

    return true;
}

/*------------------------------------test------------------------------------*/


